/**
 * @file
 * @brief Defines the interface data type for upper-level system.
 */

#ifndef _WS_CANBUS_CONTROLLER_H_
#define _WS_CANBUS_CONTROLLER_H_

#include "canbus/handler.h"
#include "canbus/macro.h"

/**
 * @namespace canbus
 * @brief canbus
 */
namespace canbus {

/**
 * @struct InterfaceData
 * @brief The struct that defines the base data type that return to upper-level controller
 */
struct InterfaceData {
public:
  InterfaceData() = default;
  virtual ~InterfaceData() = default;
  void update(); 
};

/**
 * @struct VehicleStatus
 * @brief The struct that defines the concrete data vehicle status
 */
struct VehicleStatus: public InterfaceData {
public:
  bool auto_mode;
  bool control_on;
  bool override_bypass;
  bool estop;
  bool parking_brk;
  bool gantry_running;
  bool hoist_running;   
  bool gantry_permit;

  bool container_landed;
  bool spreader_locked;
  bool spreader_unlocked;

  template <typename T>
  void update(T* d) {
    auto_mode = d->auto_mode;
    control_on = d->control_on;
    override_bypass = d->override_bypass;
    estop = d->estop;
    parking_brk = d->parking_brk;
    gantry_running = d->gantry_running;
    hoist_running = d->hoist_running;   
    gantry_permit = d->gantry_permit;
    container_landed = d->container_landed;
    spreader_locked = d->spreader_locked;
    spreader_unlocked = d->spreader_unlocked;    
  }
};

/**
 * @struct SpreaderState
 * @brief The struct that defines the concrete data spreader state
 */
struct SpreaderState: public InterfaceData {
public:
  int spreader_size;
  uint16_t hoist_height;
  uint16_t front_sprd_pos;
  uint16_t back_sprd_pos; 

  template <typename T1, typename T2>
  void update(T1* d1, T2* d2) {
    spreader_size = d2->spreader_size;
    hoist_height = d1->hoist_height;
    front_sprd_pos = d1->front_sprd_pos;
    back_sprd_pos = d1->back_sprd_pos;   
  }
};

/**
 * @struct VehicleState
 * @brief The struct that defines the concrete data vehicle state
 */
struct VehicleState: public InterfaceData {
public:
  double gantry_speed;
  double gantry_angle;
  double gantry_radius;
  double gantry_mode;

  template <typename T>
  void update(T* d) {
    gantry_speed = d->gantry_speed;
    gantry_angle = d->gantry_angle;
    gantry_radius = d->gantry_radius;
    gantry_mode = d->gantry_mode; 
  }
};

/**
 * @struct WheelEncoder
 * @brief The struct that defines the concrete data wheel encoder
 */
struct WheelEncoder: public InterfaceData {
public:
  bool wheel_direction_fl;
  bool wheel_direction_fr;
  bool wheel_direction_rl;
  bool wheel_direction_rr;

  double wheel_angle_rl;
  double wheel_angle_rr;
  double wheel_angle_rrl; 
  double wheel_angle_rrr; 
  double wheel_angle_ffl;
  double wheel_angle_ffr;
  double wheel_angle_fl; 
  double wheel_angle_fr; 

  template <typename T1, typename T2, typename T3>
  void update(T1* d1, T2* d2, T3* d3) {

    wheel_direction_fl = d1->wheel_direction_fl;
    wheel_direction_fr = d1->wheel_direction_fr;
    wheel_direction_rl = d1->wheel_direction_rl;
    wheel_direction_rr = d1->wheel_direction_rr;

    wheel_angle_ffl = d2->wheel_angle_ffl;
    wheel_angle_ffr = d2->wheel_angle_ffr;
    wheel_angle_fl = d2->wheel_angle_fl;
    wheel_angle_fr = d2->wheel_angle_fr; 

    wheel_angle_rl = d3->wheel_angle_rl;
    wheel_angle_rr = d3->wheel_angle_rr;
    wheel_angle_rrl = d3->wheel_angle_rrl;
    wheel_angle_rrr = d3->wheel_angle_rrr;
  }
};

/**
 * @struct VehicleSensor
 * @brief The struct that defines the concrete data vehicle sensor
 */
struct VehicleSensor: public InterfaceData {
public:
  bool s_down_front_right;
  bool s_down_front_left;
  bool s_down_back_right;
  bool s_down_back_left;

  double v_front_right;
  double v_back_right; 
  double v_front_left; 
  double v_back_left;

  double s_front_right;
  double s_front_mid;
  double s_front_left; 
  double s_back_right; 
  double s_back_mid;
  double s_back_left;

  double v_tier2_back_right; 
  double v_tier3_back_left; 

  template <typename T1, typename T2, typename T3, typename T4>
  void update(T1* d1, T2* d2, T3* d3, T4* d4) {

    s_down_front_right = d1->s_down_front_right;
    s_down_front_left = d1->s_down_front_left;
    s_down_back_right = d1->s_down_back_right;
    s_down_back_left = d1->s_down_back_left;

    v_front_right = d2->v_front_right;
    v_back_right = d2->v_back_right;
    v_front_left = d2->v_front_left;
    v_back_left = d2->v_back_left;

    s_front_right = d3->s_front_right;
    s_front_mid = d3->s_front_mid;
    s_front_left = d3->s_front_left;
    s_back_right = d3->s_back_right;
    s_back_mid = d4->s_back_mid;
    s_back_left = d4->s_back_left; 

    v_tier2_back_right = d4->v_tier2_back_right;
    v_tier3_back_left = d4->v_tier3_back_left;
  }
};

/**
 * @struct VehicleCmd
 * @brief The struct that defines the concrete data vehicle sensor
 */
struct VehicleCmd: public InterfaceData {
public:
  VehicleCmd():
  auto_mode(false),
  control_on(false),
  plc_reset(false),
  override_bypass(false),
  estop(false),
  parking(false),
  turn_en(false),
  gantry_en(false),
  gantry_brake(false),
  gantry_quickstop(false),
  hydraulic_back_en(false),
  hydraulic_front_en(false),
  hoist_en(false),
  spreader_en(false)
  {}  

  bool auto_mode;
  bool control_on;
  bool plc_reset;
  bool override_bypass;
  bool estop;
  bool parking;
  bool turn_en;
  bool gantry_en;
  bool gantry_brake;
  bool gantry_quickstop;
  bool hydraulic_back_en;
  bool hydraulic_front_en;
  bool hoist_en;
  bool spreader_en;
};

/**
 * @struct VehicleCtrl
 * @brief The struct that defines the concrete data vehicle sensor
 */
struct VehicleCtrl: public InterfaceData {
public:
  VehicleCtrl():
  gantry_speed(0.0),
  gantry_angle(0.0),
  gantry_mode(1)
  {}

  double gantry_speed;
  double gantry_angle;
  double gantry_mode;
};


/**
 * @struct SpreaderCtrl
 * @brief The struct that defines the concrete data vehicle sensor
 */
struct SpreaderCtrl: public InterfaceData {
public:
  SpreaderCtrl():
  front_hydraulic(kHydraulicDefault),
  back_hydraulic(kHydraulicDefault),
  hoist_height(kHoistHeightDefault),
  spreader_cmd(0),
  op_cmd(0)
  {}

  uint16_t front_hydraulic;
  uint16_t back_hydraulic;
  uint16_t hoist_height;
  uint16_t spreader_cmd;
  uint16_t op_cmd;
};

/**
 * @class CanbusManager
 * @brief The class that handles several channels and interfaces to uppler-level system
 */
class CanbusManager {
public:

  /**
   * @brief Constructor of CanbusManager
   */
  CanbusManager();

  /**
   * @brief Destructor of CanbusManager
   */
  virtual ~CanbusManager() = default;

  /**
   * @brief Initialize all the canbus handler needed
   */
  void Init();

  /**
   * @brief register all the canbus messages to specific canbus handlers
   */
  void RegisterCanMsg();

  /**
   * @brief Start running all the canbus handler needed
   */
  void Start();

  /**
   * @brief Stop running all the canbus handler needed
   */
  void Stop();

  /**
   * @brief write vehicle command message to canbus
   * @param vcmd constant pointer points to the messages.
   */
  void WriteVehicleCmd(const VehicleCmd* vcmd);

   /**
   * @brief write vehicle control message to canbus
   * @param vctrl constant pointer points to the messages.
   */ 
  void WriteVehicleCtrl(const VehicleCtrl* vctrl);

  /**
   * @brief write spreader command message to canbus
   * @param vctrl constant pointer points to the messages.
   */
  void WriteSpreaderCmd(const SpreaderCtrl* sctrl);

  /**
   * @brief read vehicle status message from canbus
   * @return pointer points to the data to be sent
   */
  VehicleStatus* ReadVehicleStatus();

  /**
   * @brief read vehicle state message from canbus
   * @return pointer points to the data to be recv
   */
  VehicleState* ReadVehicleState();

  /**
   * @brief read spreader state message from canbus
   * @return pointer points to the data to be recv
   */
  SpreaderState* ReadSpreaderState();

  /**
   * @brief read vehicle sensor message from canbus
   * @return pointer points to the data to be recv
   */
  VehicleSensor* ReadVehicleSensor();

  /**
   * @brief read wheel encoder message from canbus
   * @return pointer points to the data to be recv
   */
  WheelEncoder* ReadWheelEncoder();

private:
  CanbusHandler channel_1;
  CanbusHandler channel_2;

  VehicleStatus vehicle_status_;
  VehicleSensor vehicle_sensor_;
  VehicleState vehicle_state_;
  SpreaderState spreader_state_;
  WheelEncoder wheel_encoder_;

  DISALLOW_COPY_AND_ASSIGN(CanbusManager)
};

}

#endif // _WS_CANBUS_CONTROLLER_H_